#pragma once
#include "DinoMath.h"
#include "DVector.h"

namespace DinoMath
{
	//template <typename T> 
	class Quaternion
	{
	public:
		float x;
		float y;
		float z;
		float w;

	public:
		Quaternion(void) {}
		~Quaternion(void) {}

		Quaternion(float _x, float _y, float _z, float _w)
		{
			x = _x;
			y = _y;
			z = _z;
			w = _w;
		}

		void make_identity()
		{
			x = float(0.0);
			y = float(0.0);
			z = float(0.0);
			w = float(1.0);
		}

		void normalize()
		{
			float slength;
			float ilength;

			slength = len();
		
			if ( slength ) 
			{
				ilength = float(1 / slength);
				x *= ilength;
				y *= ilength;
				z *= ilength;
				w *= ilength;
			}
		}

		void toMatrix( float *mtx )
		{
			// q3 implementation
			float	wx, wy, wz;
			float	xx, yy, yz;
			float	xy, xz, zz;
			float	x2, y2, z2;

			x2 = x + x;
			y2 = y + y;
			z2 = z + z;

			xx = x * x2;
			xy = x * y2;
			xz = x * z2;

			yy = y * y2;
			yz = y * z2;
			zz = z * z2;

			wx = w * x2;
			wy = w * y2;
			wz = w * z2;

			mtx[0] = float(1.0) - ( yy + zz );
			mtx[4] = xy - wz;
			mtx[8] = xz + wy;

			mtx[1] = xy + wz;
			mtx[5] = float(1.0) - ( xx + zz );
			mtx[9] = yz - wx;

			mtx[2] = xz - wy;
			mtx[6] = yz + wx;
			mtx[10] = float(1.0) - ( xx + yy );
		}

		friend inline Quaternion normalize
		(
			Quaternion const & q
		)
		{
			float len = length(q);
			if(len <= float(0)) // Problem
				return Quaternion(0, 0, 0, 1);
			float oneOverLen = float(1) / len;
			return Quaternion(q.x * oneOverLen, q.y * oneOverLen, q.z * oneOverLen, q.w * oneOverLen);
		}

		friend inline Quaternion conjugate
		(
			Quaternion const & q
		)
		{
			return Quaternion(-q.x, -q.y, -q.z, q.w);
		}

		friend inline Quaternion inverse
		(
			Quaternion const & q
		)
		{
			return conjugate(q) / /*length(q)*/length(q);
		}

		friend inline float dot
		(
			Quaternion const & q1, 
			Quaternion const & q2
		)
		{
			return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w;
		}

		//template <typename T>
		friend inline Quaternion operator- 
		(
			Quaternion const & q
		)
		{
			return Quaternion(-q.x, -q.y, -q.z, -q.w);
		}

		
		//=============================================================================================
		//template <typename T> 
		friend inline Quaternion operator/ 
		(
			Quaternion const & q, 
			float const & s
		)
		{
			return Quaternion(
				q.x / s, q.y / s, q.z / s, q.w / s);
		}

		friend Quaternion operator * ( const Quaternion & lhs, const Quaternion & rhs )
		{	
			Quaternion r;

			float A, B, C, D, E, F, G, H;

			A = (lhs.w + lhs.x) * (rhs.w + rhs.x);
			B = (lhs.z - lhs.y) * (rhs.y - rhs.z);
			C = (lhs.x - lhs.w) * (rhs.y + rhs.z);
			D = (lhs.y + lhs.z) * (rhs.x - rhs.w);
			E = (lhs.x + lhs.z) * (rhs.x + rhs.y);
			F = (lhs.x - lhs.z) * (rhs.x - rhs.y);
			G = (lhs.w + lhs.y) * (rhs.w - rhs.z);
			H = (lhs.w - lhs.y) * (rhs.w + rhs.z);

			r.w = B + (-E - F + G + H) * 0.5f;
			r.x = A - ( E + F + G + H) * 0.5f; 
			r.y =-C + ( E - F + G - H) * 0.5f;
			r.z =-D + ( E - F - G + H) * 0.5f;

			return r;
		}

		inline float len()
		{
			float length;
	
			length = x * x + y * y + z * z + w * w;
			return float(sqrt( length ));
		}

		friend inline float length(Quaternion const & q)
		{
			float length;
	
			length = q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w;
			return float(sqrt( length ));
		}

		void fromAxisAngle(float *axis, float theta)
		{
			// square norm of axis
			float sqnorm = axis[0]*axis[0] + axis[1]*axis[1] + axis[2]*axis[2];

			if (sqnorm == float(0.0))
			{
				// axis too small.
				x = y = z = float(0.0);
				w = float(1.0);
			} 
			else 
			{
				theta *= float(0.5);
				float sin_theta = float(sin(theta));

				if ( sqnorm != float(1)) 
					sin_theta /= float(sqrt(sqnorm));
				x = sin_theta * axis[0];
				y = sin_theta * axis[1];
				z = sin_theta * axis[2];
				w = float(cos(theta));
			}
		}
		/*
		template <typename T> 
		inline Quaternion operator*
		( 
			Quaternion const & q, 
			Quaternion const & p
		) 
		{ 
			return Quaternion(
				q.w * p.w - q.x * p.x - q.y * p.y - q.z * p.z,
				q.w * p.x + q.x * p.w + q.y * p.z - q.z * p.y,
				q.w * p.y + q.y * p.w + q.z * p.x - q.x * p.z,
				q.w * p.z + q.z * p.w + q.x * p.y - q.y * p.x);
		} 
		*/
		// Transformation
		//template <typename T>
		friend inline vec3 operator* 
		(
			Quaternion const & q, 
			vec3 const & v
		)
		{
			float Two(2);

			vec3 uv, uuv;
			vec3 QuatVector(q.x, q.y, q.z);
			DinoMath::CrossProduct3(uv, QuatVector, v);
			DinoMath::CrossProduct3(uuv, QuatVector, uv);
			//uv = glm::cross(QuatVector, v);
			//uuv = glm::cross(QuatVector, uv);
			uv *= (Two * q.w); 
			uuv *= Two; 

			return v + uv + uuv;
		}

		//template <typename T>
		friend inline vec3 operator* 
		(
			vec3 const & v,
			Quaternion const & q 
		)
		{
			return inverse(q) * v;
		}


		/*
		template <typename T>
		inline detail::tvec4<T> operator* 
		(
			Quaternion const & q, 
			detail::tvec4<T> const & v
		)
		{
			return detail::tvec4<T>(q * detail::tvec3<T>(v), v.w);
		}

		template <typename T>
		inline detail::tvec4<T> operator* 
		(
			detail::tvec4<T> const & v,
			Quaternion const & q 
		)
		{
			return gtc::quaternion::inverse(q) * v;
		}

		template <typename T> 
		inline Quaternion operator* 
		(
			Quaternion const & q, 
			float const & s
		)
		{
			return Quaternion(
				q.w * s, q.x * s, q.y * s, q.z * s);
		}

		template <typename T> 
		inline Quaternion operator* 
		(
			float const & s,
			Quaternion const & q
		)
		{
			return q * s;
		}*/

	};


	//typedef Quaternion Quaternionf;
}